#ifndef __ROB_CONTROL_H__
#define __ROB_CONTROL_H__

#include "rob_common.h"
#include "rob_kinematics.h"
#include "rob_servo.h"
#include "rob_sensor.h"
#include "rob_communication.h"
#include "mfl.h"  /* Include EASML for trajectory planning */
#include "ifl.h"  /* Include EASML interpolation library */

/* System Configuration */
#define ROB_CONTROL_CYCLE_TIME_MS 20u    /* Control cycle time (ms) */
#define ROB_MAX_ACTIONS 50u              /* Maximum stored actions */
#define ROB_MAX_SEQUENCE_POINTS 100u     /* Maximum sequence points */

/* Control Modes */
typedef enum {
    ROB_CTRL_MODE_MANUAL = 0u,      /* Manual control via commands */
    ROB_CTRL_MODE_AUTO = 1u,        /* Automatic sequence execution */
    ROB_CTRL_MODE_LEARNING = 2u,    /* Learning mode - record actions */
    ROB_CTRL_MODE_REPLAY = 3u,      /* Replay recorded actions */
    ROB_CTRL_MODE_KINEMATICS = 4u,  /* Direct kinematics control */
    ROB_CTRL_MODE_EMERGENCY = 5u    /* Emergency stop mode */
} Rob_ControlMode_Type;

/* Action Types */
typedef enum {
    ROB_ACTION_SERVO_MOVE = 0u,
    ROB_ACTION_SERVO_MOVE_ALL = 1u,
    ROB_ACTION_DELAY = 2u,
    ROB_ACTION_KINEMATICS_MOVE = 3u,
    ROB_ACTION_GRIPPER_OPEN = 4u,
    ROB_ACTION_GRIPPER_CLOSE = 5u,
    ROB_ACTION_WAIT_SENSOR = 6u,
    ROB_ACTION_BEEP = 7u,
    ROB_ACTION_LED_CONTROL = 8u
} Rob_ActionType_Type;

/* Action Structure */
typedef struct {
    Rob_ActionType_Type Type;
    union {
        struct {
            uint8 ServoIndex_u8;
            float32 TargetAngle_f32;
            uint16 Duration_u16;
        } ServoMove;
        
        struct {
            float32 Angles_af32[ROB_SERVO_NUM];
            uint16 Duration_u16;
        } ServoMoveAll;
        
        struct {
            uint16 DelayTime_u16;
        } Delay;
        
        struct {
            float32 X_f32;
            float32 Y_f32;
            float32 Z_f32;
            float32 Alpha_f32;
            float32 Beta_f32;
            uint16 Duration_u16;
        } KinematicsMove;
        
        struct {
            Rob_SensorType_Type SensorType;
            float32 Threshold_f32;
            uint32 Timeout_u32;
        } WaitSensor;
        
        struct {
            uint16 Frequency_u16;
            uint16 Duration_u16;
        } Beep;
        
        struct {
            uint8 LedIndex_u8;
            boolean State_b;
        } LedControl;
    } Parameters;
    
    uint32 StartTime_u32;
    boolean IsCompleted_b;
    Rob_Status_Type Status;
} Rob_Action_Type;

/* Action Sequence Structure */
typedef struct {
    Rob_Action_Type Actions_ast[ROB_MAX_ACTIONS];
    uint16 ActionCount_u16;
    uint16 CurrentAction_u16;
    boolean IsActive_b;
    boolean IsLooping_b;
    uint32 StartTime_u32;
    Rob_Status_Type Status;
} Rob_ActionSequence_Type;

/* Trajectory Planning Structure (using EASML interpolation) */
typedef struct {
    float32 StartPos_af32[ROB_SERVO_NUM];
    float32 EndPos_af32[ROB_SERVO_NUM];
    float32 CurrentPos_af32[ROB_SERVO_NUM];
    float32 Velocity_af32[ROB_SERVO_NUM];
    float32 Acceleration_af32[ROB_SERVO_NUM];
    uint32 Duration_u32;
    uint32 StartTime_u32;
    uint32 CurrentTime_u32;
    boolean IsActive_b;
    Rob_Status_Type Status;
} Rob_TrajectoryPlanner_Type;

/* Safety System Structure */
typedef struct {
    boolean EmergencyStop_b;
    boolean SoftLimitsEnabled_b;
    boolean CollisionDetection_b;
    float32 MaxVelocity_af32[ROB_SERVO_NUM];
    float32 MaxAcceleration_af32[ROB_SERVO_NUM];
    uint32 WatchdogTimeout_u32;
    uint32 LastWatchdogReset_u32;
    Rob_Status_Type Status;
} Rob_SafetySystem_Type;

/* Control System State */
typedef struct {
    Rob_ControlMode_Type CurrentMode;
    Rob_ControlMode_Type PreviousMode;
    Rob_State_Type SystemState;
    
    /* Subsystem instances */
    Rob_KinState_Type Kinematics;
    Rob_ServoSystem_Type ServoSystem;
    Rob_SensorSystem_Type SensorSystem;
    Rob_CommContext_Type Communication;
    
    /* Control structures */
    Rob_ActionSequence_Type CurrentSequence;
    Rob_TrajectoryPlanner_Type Trajectory;
    Rob_SafetySystem_Type Safety;
    
    /* Timing */
    uint32 SystemStartTime_u32;
    uint32 LastControlUpdate_u32;
    uint32 ControlCycleCount_u32;
    
    /* Status */
    Rob_Status_Type OverallStatus;
    uint8 ErrorCode_u8;
    char ErrorMessage_ac8[64];
    
} Rob_ControlSystem_Type;

/* Learning Mode Structure */
typedef struct {
    Rob_ActionSequence_Type RecordedSequence;
    boolean IsRecording_b;
    uint32 LastRecordTime_u32;
    uint16 RecordThreshold_u16;  /* Minimum movement to record */
    Rob_Status_Type Status;
} Rob_LearningMode_Type;

/* Function Declarations */

/* System Initialization and Management */
Rob_Status_Type Rob_ControlInit(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlStart(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlStop(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlReset(Rob_ControlSystem_Type *System_pst);

/* Main Control Loop */
Rob_Status_Type Rob_ControlUpdate(Rob_ControlSystem_Type *System_pst,
                                 uint32 CurrentTime_u32);

Rob_Status_Type Rob_ControlProcess(Rob_ControlSystem_Type *System_pst);

/* Mode Management */
Rob_Status_Type Rob_ControlSetMode(Rob_ControlSystem_Type *System_pst,
                                  Rob_ControlMode_Type NewMode_e);

Rob_ControlMode_Type Rob_ControlGetMode(const Rob_ControlSystem_Type *System_cpst);

/* Action Management */
Rob_Status_Type Rob_ControlAddAction(Rob_ActionSequence_Type *Sequence_pst,
                                    const Rob_Action_Type *Action_cpst);

Rob_Status_Type Rob_ControlExecuteAction(Rob_ControlSystem_Type *System_pst,
                                        const Rob_Action_Type *Action_cpst);

Rob_Status_Type Rob_ControlProcessActions(Rob_ControlSystem_Type *System_pst,
                                         uint32 CurrentTime_u32);

/* Sequence Control */
Rob_Status_Type Rob_ControlStartSequence(Rob_ControlSystem_Type *System_pst,
                                        const Rob_ActionSequence_Type *Sequence_cpst);

Rob_Status_Type Rob_ControlStopSequence(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlPauseSequence(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlResumeSequence(Rob_ControlSystem_Type *System_pst);

/* Trajectory Planning (using EASML interpolation) */
Rob_Status_Type Rob_ControlTrajectoryInit(Rob_TrajectoryPlanner_Type *Planner_pst);

Rob_Status_Type Rob_ControlTrajectoryPlan(Rob_TrajectoryPlanner_Type *Planner_pst,
                                         const float32 StartPos_cpf32[ROB_SERVO_NUM],
                                         const float32 EndPos_cpf32[ROB_SERVO_NUM],
                                         uint32 Duration_u32);

Rob_Status_Type Rob_ControlTrajectoryUpdate(Rob_TrajectoryPlanner_Type *Planner_pst,
                                           uint32 CurrentTime_u32,
                                           float32 CurrentPos_pf32[ROB_SERVO_NUM]);

/* Safety System */
Rob_Status_Type Rob_ControlSafetyInit(Rob_SafetySystem_Type *Safety_pst);

Rob_Status_Type Rob_ControlSafetyUpdate(Rob_SafetySystem_Type *Safety_pst,
                                       const Rob_ControlSystem_Type *System_cpst);

Rob_Status_Type Rob_ControlEmergencyStop(Rob_ControlSystem_Type *System_pst);

boolean Rob_ControlCheckSafety_b(const Rob_SafetySystem_Type *Safety_cpst);

/* Learning Mode Functions */
Rob_Status_Type Rob_ControlLearningStart(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlLearningStop(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlLearningRecord(Rob_ControlSystem_Type *System_pst,
                                         uint32 CurrentTime_u32);

/* High-Level Control Functions */
Rob_Status_Type Rob_ControlMoveToPosition(Rob_ControlSystem_Type *System_pst,
                                         float32 X_f32, float32 Y_f32, float32 Z_f32,
                                         float32 Alpha_f32, float32 Beta_f32,
                                         uint32 Duration_u32);

Rob_Status_Type Rob_ControlMoveServo(Rob_ControlSystem_Type *System_pst,
                                    uint8 ServoIndex_u8, float32 Angle_f32,
                                    uint32 Duration_u32);

Rob_Status_Type Rob_ControlMoveAllServos(Rob_ControlSystem_Type *System_pst,
                                        const float32 Angles_cpf32[ROB_SERVO_NUM],
                                        uint32 Duration_u32);

/* Status and Monitoring */
Rob_Status_Type Rob_ControlGetStatus(const Rob_ControlSystem_Type *System_cpst);

Rob_State_Type Rob_ControlGetState(const Rob_ControlSystem_Type *System_cpst);

boolean Rob_ControlIsIdle_b(const Rob_ControlSystem_Type *System_cpst);

boolean Rob_ControlIsMoving_b(const Rob_ControlSystem_Type *System_cpst);

/* Calibration and Configuration */
Rob_Status_Type Rob_ControlCalibrate(Rob_ControlSystem_Type *System_pst);

Rob_Status_Type Rob_ControlLoadConfiguration(Rob_ControlSystem_Type *System_pst,
                                            const void *Config_cpv);

Rob_Status_Type Rob_ControlSaveConfiguration(const Rob_ControlSystem_Type *System_cpst,
                                            void *Config_pv);

/* Utility Functions */
uint32 Rob_ControlGetUptime_u32(const Rob_ControlSystem_Type *System_cpst,
                               uint32 CurrentTime_u32);

float32 Rob_ControlGetCycleTime_f32(const Rob_ControlSystem_Type *System_cpst);

void Rob_ControlSetError(Rob_ControlSystem_Type *System_pst,
                        uint8 ErrorCode_u8, const char *ErrorMessage_cpc8);

void Rob_ControlClearError(Rob_ControlSystem_Type *System_pst);

#endif /* __ROB_CONTROL_H__ */
